/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * Copyright (c) 2006-2025, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-06-05     RT-Thread    first version
 */
#include <rtthread.h>
#include "drv_serial.h"
#include "drv_i2s.h"
#include "w25qxx.h"
#include "usb_conf.h"
#include "drv_spim.h"
extern int fc_task_thread_init(void);
// double atof(const char *s)
// {
//     double power, value;
//     int i = 0;
//     int sign = 0;
//     // assert(s!=NULL);//判断字符串是否为空
//     // for(i=0;is_space(s[i]);i++);//除去字符串前的空格
//     sign = (s[i] == '-') ? -1 : 1;
//     if (s[i] == '-' || s[i] == '+') // 要是有符号位就前进一位
//         i++;
//     for (value = 0.0; isdigit((int)s[i]); i++) // 计算小数点前的数字
//         value = value * 10.0 + (s[i] - '0');
//     if (s[i] == '.')
//         i++;
//     for (power = 1.0; isdigit((int)s[i]); i++) // 计算小数点后的数字
//     {
//         value = value * 10.0 + (s[i] - '0');
//         power *= 10.0;
//     }
//     return sign * value / power;
// }
void memory_test(void)
{
    char *p;
    for (int i = 20; i < 64; i++)
    {
        p = (char *)(i * 1024 + 0x20000000);

        *p = 0x55;
        if (*p != 0x55)
        {
            rt_kprintf("memory test fail\n");
        }
        else
        {
            rt_kprintf("memory test %2dKb 0x%08X\n", i, (int)p);
        }
        rt_thread_delay(20);
    }
}
MSH_CMD_EXPORT(memory_test, memory test);
uint8_t rx4_buffer[512];
void printf_fl(void)
{
    for (int i = 0; i < 1024; i++)
    {
        sector_read(SPIM_TEST_ADDR + i * 512, 512,rx4_buffer);
        uart1_write(rx4_buffer, 512);
        rt_thread_mdelay(1);
    }
}
MSH_CMD_EXPORT(printf_fl, printf flash);
// uint8_t rx4_buffer[256 + 9];
int main(void)
{
    // int count = 1;

    // fc_task_thread_init();
    rt_hw_uart4_init();
    i2s_dma_init();
    spim_init();
    usb_main_init();
    uint16_t cnt = 0;
    // spim_all();

    while (1)
    {
        // int rxlen4 = vcp_get_rxdata(rx4_buffer);
        int rxlen4 = usart1_get_rx_data_len();
        if (rxlen4 == 2 || rxlen4 >=9)
        {
            // uart1_read(rx4_buffer, rxlen4);
            // get_data_task(rxlen4, rx4_buffer);
        }
        // count++;
        // rt_kprintf("1234 %d\n", rt_tick_get());
        // LOG_D("Hello RT-Thread! %ld", rt_tick_get());
        // rt_kprintf("main %d\n", rt_tick_get());
        // adc_show_value();
        // uart4_write("hello\n", 5);
        if (cnt++ > 100)
        {
            // rt_kprintf("main %d\n", rt_tick_get());
            cnt = 0;
        }
        rt_thread_mdelay(35);
    }

    return RT_EOK;
}

#if 0
void omega2_filter(void)
{

    float height, dd_height, _climb_rate, _hgtCompFiltOmega;
    /*
          use a complimentary filter to calculate climb_rate. This is
          designed to minimise lag
         */
    float baro_alt = _ahrs.get_baro().get_altitude();
    // Get height acceleration
    float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
    // Perform filter calculation using backwards Euler integration
    // Coefficients selected to place all three filter poles at omega
    float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
    float hgt_err = baro_alt - height;
    float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;

    dd_height += integ1_input * DT;

    float integ2_input = dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;

    _climb_rate += integ2_input * DT;

    float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
    // If more than 1 second has elapsed since last update then reset the integrator state
    // to the measured height
    if (DT > 1.0f)
    {
        height = _height;
    }
    else
    {
        height += integ3_input * DT;
    }
}

#define TIME_CONTANST_Z 5.0f
#define K_ACC_Z (5.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_VEL_Z (3.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_POS_Z (3.0f / TIME_CONTANST_Z)

float High_Filter[3] = {
    0.03, // 0.015
    0.05, // 0.05
    0.02  // 0.03
};

float Altitude_Dealt = 0;
void Strapdown_INS_High()
{
    // Altitude_Dealt=HC_SR04_Distance-NamelessQuad.Position[_YAW];//气压计(超声波)与SINS估计量的差，单位cm
    Altitude_Dealt = Altitude_Estimate - NamelessQuad.Position[_YAW]; // 气压计(超声波)与SINS估计量的差，单位cm

    acc_correction[_YAW] += High_Filter[0] * Altitude_Dealt * K_ACC_Z; // 加速度校正量
    vel_correction[_YAW] += High_Filter[1] * Altitude_Dealt * K_VEL_Z; // 速度校正量
    pos_correction[_YAW] += High_Filter[2] * Altitude_Dealt * K_POS_Z; // 位置校正量

    // 原始加速度+加速度校正量=融合后的加速度
    NamelessQuad.Acceleration[_YAW] = Origion_NamelessQuad.Acceleration[_YAW] + acc_correction[_YAW];

    // 融合后的加速度积分得到速度增量
    SpeedDealt[_YAW] = NamelessQuad.Acceleration[_YAW] * CNTLCYCLE;

    // 得到速度增量后，更新原始位置
    Origion_NamelessQuad.Position[_YAW] += (NamelessQuad.Speed[_YAW] + 0.5 * SpeedDealt[_YAW]) * CNTLCYCLE;

    // 原始位置+位置校正量=融合后位置
    NamelessQuad.Position[_YAW] = Origion_NamelessQuad.Position[_YAW] + pos_correction[_YAW];

    // 原始速度+速度校正量=融合后的速度
    Origion_NamelessQuad.Speed[_YAW] += SpeedDealt[_YAW];
    NamelessQuad.Speed[_YAW] = Origion_NamelessQuad.Speed[_YAW] + vel_correction[_YAW];
}
#endif
